#include <signal.h>
#include <std_msgs/String.h>
#include "ros/ros.h"
#include <geometry_msgs/Twist.h> 
#include <webots_ros/set_float.h>
#include <webots_ros/set_int.h>
#include <webots_ros/Int32Stamped.h>
#include <math.h>
using namespace std;

#define TIME_STEP 5    //时钟
#define NMOTORS 2       //电机数量
#define MAX_SPEED 1000   //电机最大速度

ros::NodeHandle *n;

static int controllerCount;
static std::vector<std::string> controllerList; 

ros::ServiceClient timeStepClient;          //时钟通讯客户端
webots_ros::set_int timeStepSrv;            //时钟服务数据

ros::ServiceClient set_velocity_client;
webots_ros::set_float set_velocity_srv;

ros::ServiceClient set_position_client;   
webots_ros::set_float set_position_srv;   

double speeds[NMOTORS]={MAX_SPEED,0.0},pos[NMOTORS]={0.0,INFINITY};// 2电机速度值 0～10
double l=0.18,r=0.025;

string model_name="my_robot";
// 控制位置电机
static const char *motorNames[NMOTORS] ={"motor1", "motor2"};

/*******************************************************
* Function name ：updateSpeed
* Description   ：将速度请求以set_float的形式发送给set_velocity_srv
* Parameter     ：无
* Return        ：无
**********************************************************/
void updateSpeed() {   
    ROS_INFO("motor1_pos:%lf,  motor2_vel:%lf", pos[0], speeds[1]);
    for (int i = 0; i < NMOTORS; ++i) {
        // 更新速度,position
        set_velocity_client = n->serviceClient<webots_ros::set_float>(string("/my_robot/") + string(motorNames[i]) + string("/set_velocity"));   
        set_velocity_srv.request.value = speeds[i];
        set_velocity_client.call(set_velocity_srv);

        set_position_client = n->serviceClient<webots_ros::set_float>(string("/my_robot/") + string(motorNames[i]) + string("/set_position"));   
        set_position_srv.request.value = pos[i];
        set_position_client.call(set_position_srv);

    }
}

/*******************************************************
* Function name ：controllerNameCallback
* Description   ：控制器名回调函数，获取当前ROS存在的机器人控制器
* Parameter     ：
        @name   控制器名
* Return        ：无
**********************************************************/
// catch names of the controllers availables on ROS network
void controllerNameCallback(const std_msgs::String::ConstPtr &name) { 
    controllerCount++; 
    controllerList.push_back(name->data);//将控制器名加入到列表中
    ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());

}

/*******************************************************
* Function name ：quit
* Description   ：退出函数
* Parameter     ：
        @sig   信号
* Return        ：无
**********************************************************/
void quit(int sig) {
    ROS_INFO("User stopped the '/my_robot' node.");
    timeStepSrv.request.value = 0; 
    timeStepClient.call(timeStepSrv); 
    ros::shutdown();
    exit(0);
}
/*******************************************************
* Function name ：键盘返回函数
* Description   ：当键盘动作，就会进入此函数内
* Parameter     ：
        @value   返回的值
* Return        ：无
**********************************************************/
void keyboardDataCallback(const webots_ros::Int32Stamped::ConstPtr &value)
{
    cout<<value->data;
    switch (value->data)
    {
        case 314:
            pos[0]=-M_PI/2;
            speeds[1]= 2;
            break;
        case 315:
            pos[0]=0;
            speeds[1]= 5;
            break;
        case 316:
            pos[0]=M_PI/2;
            speeds[1]= 2;
            break;
        case 317:
            pos[0]=0;
            speeds[1]= 5;
            break;
        case 32:
            pos[0]=0;
            speeds[1]= 0;
            break;
        default:
            break;
    }
    updateSpeed();
}

void cmdvelDataCallback(const geometry_msgs::Twist::ConstPtr &value)
{
    double linear_temp=0, angular_temp=0;//暂存的线速度和角速度,
    angular_temp = value->angular.z ;//获取/cmd_vel的角速度,rad/s
    linear_temp = value->linear.x ;//获取/cmd_vel的线速度.m/s

    if (linear_temp==0)
    {
        if (angular_temp>0)
        {
            pos[0]=-M_PI/2;
        }
        if (angular_temp<0)
        {
            pos[0]=M_PI/2;
        }
        speeds[1]  = angular_temp*l/r; 
    }
    if (angular_temp==0)
    {
        pos[0]=0;
        speeds[1]  = linear_temp/r; 
    }
    if (linear_temp!=0&&angular_temp!=0)
    {
        if (linear_temp>0){
        double r_temp=fabs(linear_temp/angular_temp);
        pos[0]=-atan(l/r_temp);
        speeds[1]  = sqrt(r_temp*r_temp+l*l)/(r_temp*r); 
        }
        if (linear_temp<0){
        double r_temp=fabs(linear_temp/angular_temp);
        pos[0]=atan(l/r_temp);
        speeds[1]  = sqrt(r_temp*r_temp+l*l)/(r_temp*r); 
        }

    }

    updateSpeed();
    // ROS_INFO("motor1_pos:%lf,  motor2_vel:%lf", pos[0], speeds[1]);
}


int main(int argc, char **argv) {

    std::string controllerName;
    // create a node named 'robot' on ROS network
    ros::init(argc, argv, "move_robot", ros::init_options::AnonymousName);
    n = new ros::NodeHandle;

    signal(SIGINT, quit);

    // subscribe to the topic model_name to get the list of availables controllers
    ros::Subscriber nameSub = n->subscribe("model_name", 100, controllerNameCallback);
    while (controllerCount == 0 || controllerCount < nameSub.getNumPublishers()) {
        ros::spinOnce();
    }
    ros::spinOnce();

    timeStepClient = n->serviceClient<webots_ros::set_int>("my_robot/robot/time_step");
    timeStepSrv.request.value = TIME_STEP;

    // if there is more than one controller available, it let the user choose
    if (controllerCount == 1)
        controllerName = controllerList[0];
    else {
        int wantedController = 0;
        std::cout << "Choose the # of the controller you want to use:\n";
        std::cin >> wantedController;
        if (1 <= wantedController && wantedController <= controllerCount)
        controllerName = controllerList[wantedController - 1];
        else {
        ROS_ERROR("Invalid number for controller choice.");
        return 1;
        }
    }
    ROS_INFO("Using controller: '%s'", controllerName.c_str());
    // leave topic once it is not necessary anymore
    nameSub.shutdown();

    // init motors 
    
    set_position_client = n->serviceClient<webots_ros::set_float>(string("/my_robot/") + string(motorNames[0]) + string("/set_position"));   
    set_position_srv.request.value = 0;
    if (set_position_client.call(set_position_srv) && set_position_srv.response.success)     
        ROS_INFO("Position set to 0 for motor %s.", motorNames[0]);   
    else     
        ROS_ERROR("Failed to call service set_position on motor %s.", motorNames[0]);  
   
    set_position_client = n->serviceClient<webots_ros::set_float>(string("/my_robot/") + string(motorNames[1]) + string("/set_position"));   
    set_position_srv.request.value = INFINITY;
    if (set_position_client.call(set_position_srv) && set_position_srv.response.success)     
        ROS_INFO("Position set to 0 for motor %s.", motorNames[1]);   
    else     
        ROS_ERROR("Failed to call service set_position on motor %s.", motorNames[1]); 


    set_velocity_client = n->serviceClient<webots_ros::set_float>(string("/my_robot/") + string(motorNames[0]) + string("/set_velocity"));   
    set_velocity_srv.request.value = MAX_SPEED;   
    if (set_velocity_client.call(set_velocity_srv) && set_velocity_srv.response.success == 1)     
        ROS_INFO("Velocity set to MAX_SPEED for motor %s.", motorNames[0]);   
    else     
        ROS_ERROR("Failed to call service set_velocity on motor %s.", motorNames[0]); 
    
    set_velocity_client = n->serviceClient<webots_ros::set_float>(string("/my_robot/") + string(motorNames[1]) + string("/set_velocity"));   
    set_velocity_srv.request.value = 0;   
    if (set_velocity_client.call(set_velocity_srv) && set_velocity_srv.response.success == 1)     
        ROS_INFO("Velocity set to 0 for motor %s.", motorNames[1]);   
    else     
        ROS_ERROR("Failed to call service set_velocity on motor %s.", motorNames[1]); 

     // 服务订阅键盘
    ros::ServiceClient keyboardEnableClient;
    webots_ros::set_int keyboardEnablesrv;
    ros::Subscriber keyboardSub;
    keyboardEnableClient = n->serviceClient<webots_ros::set_int>(model_name+"/keyboard/enable");
    keyboardEnablesrv.request.value = TIME_STEP;
    if (keyboardEnableClient.call(keyboardEnablesrv) && keyboardEnablesrv.response.success)
    {
        
        keyboardSub = n->subscribe(model_name+"/keyboard/key",1,keyboardDataCallback);
        while (keyboardSub.getNumPublishers() == 0) {}
        setlocale(LC_CTYPE,"zh_CN.utf8");//设置中文
        ROS_INFO("Keyboard enabled.");
        ROS_INFO("控制方向：");
        ROS_INFO("  ↑  ");
        ROS_INFO("← ↓ →");
        ROS_INFO("刹车：空格键");
        ROS_INFO("Use the arrows in Webots window to move the robot.");
        ROS_INFO("Press the End key to stop the node.");
    }
    else
    {
        ROS_ERROR("Could not enable keyboard, success = %d.", keyboardEnablesrv.response.success);
        return 1;
    }

    //subscribe move_base
    ros::Subscriber cmdvelSub;
    cmdvelSub = n->subscribe("/cmd_vel",1,cmdvelDataCallback);
    while (cmdvelSub.getNumPublishers() == 0) {}    

    //main loop
    ros::Rate loop(100);
    while (ros::ok()) {  
        ROS_INFO("1"); 
        ros::spinOnce();
        // ROS_INFO("2");
        // if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success)
        // {  
        //     ROS_ERROR("Failed to call service time_step for next step.");     
        //     // break;   
        // }   
        ROS_INFO("ok.");
        loop.sleep(); 
        // ros::spinOnce();
    } 

    timeStepSrv.request.value = 0;
    timeStepClient.call(timeStepSrv);
    ros::shutdown(); 
    return 0;
}

